#include <EEPROM.h>

/* Open Source 360Â° continuous rotation antenna tracker software
 * created by Samuel Brucksch
 *
 * Digital Smooth method from Arduino playground: http://playground.arduino.cc/Main/DigitalSmooth
 * Local GPS uses parser from TinyGPS: http://arduiniana.org/libraries/tinygps/
 * FPV Community thread: http://fpv-community.de/showthread.php?46439-360%B0-Community-Antennentracker-mit-Schleifring-Wer-macht-mit
 */
#include "defines.h"
#include "config.h"
#include <Wire.h>
#include "compass.h"
#include "servos.h"
#include "inttypes.h"
#include "telemetry.h"
#include "uart.h"


void initGps();
// inlined as it gets called from only one location
void calcPan() __attribute__((always_inline));
// inlined as it gets called from only one location
void calcTilt() __attribute__((always_inline));
int digitalSmooth(int rawIn, int *sensSmoothArray);
uint16_t getHeading(geoCoordinate_t *a, geoCoordinate_t *b) __attribute__((always_inline));


#ifdef LOCAL_GPS
#include <SoftwareSerial.h>
SoftwareSerial gpsSerial(GPS_RX_PIN, GPS_TX_PIN);
#include <TinyGPS.h>
TinyGPS gps;
#endif


//distance in meter
uint16_t distance;
// The target position (lat/lon)
geoCoordinate_t targetPosition;
// The tracker position (lat/lon)
geoCoordinate_t trackerPosition;

#define filterSamples 10
int currentHeadingSmoothArray [filterSamples];
int targetHeadingSmoothArray [filterSamples];
int altSmoothArray [filterSamples];
int distanceSmoothArray[filterSamples];

uint32_t time;
long calib_timer;

void setup(){
  hasLat = false;
  hasLon = false;
  hasAlt = false;
  HOME_SET = false;
  PREVIOUS_STATE = true;
  TRACKING_STARTED = false;
  CURRENT_STATE = true;
  gotNewHeading = false;

  cli(); 
    initUart();
  sei(); 
  #ifdef DEBUG
    uart_puts("Setup start\n");
  #endif
  Wire.begin();
  
  pinMode(LED_PIN, OUTPUT);
  pinMode(HOME_BUTTON, INPUT);
  pinMode(CALIB_BUTTON, INPUT);

  //LED off
  digitalWrite(LED_PIN, LOW);
  
  //enable internal pullups
  digitalWrite(HOME_BUTTON, HIGH);
  digitalWrite(CALIB_BUTTON, HIGH);

  // init pan/tilt servos controlled via hardware pwm
  initServos();
  initCompass();
  
#ifdef LOCAL_GPS
  gpsSerial.begin(GPS_BAUDRATE);
  //let gps serial initialize before we configure it
  delay(20);
  initGps();
#endif

  
  #ifdef DEBUG
    uart_puts("Setup finished\n");
  #endif
  time = millis();
}


void loop(){  

  uint8_t c;
  if (uart_get_char(c))
  {
    encodeTargetData(c);
  }

  if (hasAlt){
    targetPosition.alt = digitalSmooth(getTargetAlt(), altSmoothArray);
    #ifdef DEBUG
      char s[10];
      uart_puts("Target alt: ");uart_puts(itoa(targetPosition.alt, s, 10));
    #endif
    hasAlt = false;
  }
  
  //only calculate distance and heading when we have valid telemetry data
  if (hasLat && hasLon){
    targetPosition.lat = getTargetLat();
    targetPosition.lon = getTargetLon();    
    // calculate distance without haversine. We need this for the slope triangle to get the correct pan value
    distance = sqrt( (targetPosition.lat - trackerPosition.lat) * (targetPosition.lat - trackerPosition.lat) 
                      + (targetPosition.lon - trackerPosition.lon) * (targetPosition.lon - trackerPosition.lon) );
    distance = digitalSmooth(distance, distanceSmoothArray);
    targetPosition.heading = digitalSmooth(getHeading(&trackerPosition, &targetPosition), targetHeadingSmoothArray);
    #ifdef DEBUG
      char s[10];
      // TODO correct debug output for lat/lon
      uart_puts("Lat: ");uart_puts(dtostrf(targetPosition.lat , 8, 5, s ));uart_puts(" Lon: ");uart_puts(dtostrf(targetPosition.lon , 7, 5, s ));
      uart_puts(" Distance: ");uart_puts(itoa(distance, s, 10));uart_puts(" Heading: ");uart_puts(itoa(trackerPosition.heading, s, 10));uart_puts(" Target Heading: ");uart_puts(itoa(targetPosition.heading, s, 10));
    #endif
    hasLat = false;
    hasLon = false;
    gotNewHeading = true;
  }
  
  // TODO we should change this to a 50hz loop here, read the compass and update the servos
  // afterwards. 
  // As a result there will be no overhead in reading compass values and the compass is read
  // when it makes sense, because the servos can only be refreshed at 50hz.
  
  // refresh rate of compass is 75Hz -> 13.333ms to refresh the data
  // we update the heading every 14ms to get as many samples into the smooth array as possible
  if (millis() > time){
    time = millis() + 14;
    trackerPosition.heading = digitalSmooth(getHeading(), currentHeadingSmoothArray);
    gotNewHeading = true;
      #ifdef DEBUG
        char s[10];
        uart_puts(itoa(trackerPosition.heading,s,10));uart_puts("\n");
      #endif
  }
  
  CURRENT_STATE = digitalRead(CALIB_BUTTON);
  if (CURRENT_STATE != PREVIOUS_STATE){
    digitalWrite(LED_PIN, !CURRENT_STATE);
     //pin changed
     if (!CURRENT_STATE && calib_timer == 0){
        calib_timer = millis();
     } else if (CURRENT_STATE && millis() - calib_timer < 4000){
       //button not pressed long enough
       calib_timer = 0; 
     } else if (CURRENT_STATE && millis() - calib_timer > 4000){
       //start calibration routine if button pressed > 4s and released
       //cli();
       startCompassCalibration();
       calib_timer = 0; 
       //sei();
     }
     PREVIOUS_STATE = CURRENT_STATE;
  }

#ifndef LOCAL_GPS
  //only needed if no local gps
  if (!digitalRead(HOME_BUTTON)){
    //set home
    trackerPosition.lat = getTargetLat();
    trackerPosition.lon = getTargetLon();
    trackerPosition.alt = digitalSmooth(getTargetAlt(), altSmoothArray);
    HOME_SET = true;
  }
#endif

#ifdef LOCAL_GPS
  if (gpsSerial.available()){
    uint8_t c = gpsSerial.read();
    #ifdef DEBUG
      uart_putc(c);
    #endif
    if (gps.encode(c)){
      gps.get_position(&trackerPosition.lat,&trackerPosition.lon);
      digitalWrite(LED_PIN, HIGH);
      if(gps.altitude() != TinyGPS::GPS_INVALID_ALTITUDE) {
        trackerPosition.alt = (int16_t)gps.altitude();
      }
    }
    else{
      digitalWrite(LED_PIN, LOW); 
    }
  }
#endif

#if (START_TRACKING_DISTANCE > 0)
  //Only track if tracking process started.
  if(!TRACKING_STARTED) {
    //if plane is START_TRACKING_DISTANCE meter away from tracker start tracking process.
    if (START_TRACKING_DISTANCE <= distance && HOME_SET){
      TRACKING_STARTED = true;
    }
  } else {
#else
  //Only track if home ist set.
  if (HOME_SET){
#endif
    digitalWrite(LED_PIN, HIGH);
    // only update pan value if there is new data
    if( gotNewHeading ) {
      calcPan();
      gotNewHeading = false;
    }
    calcTilt(); 
  } 
}

void calcPan(){
  int16_t delta = targetPosition.heading - trackerPosition.heading;

  if (delta > 180){
    delta -= 360; 
  } 
  else if (delta < -180){
    delta += 360;
  }

  // Because delta moves between +/-180 and with 5 as multiplicator
  // the max for delta is +/-900. So we don't need a min/max
  // check as speed will be in the range of 600 to 2400
  int16_t speed = PAN_0;
  if (abs(delta) > 1){
    speed = PAN_0 + (delta * 40);
  }

#if 1
  // when using a multiplicator > 5 you will have to
  // use this code here to avoid range overflows
  PAN_SERVO = constrain(speed,500,2500)*2;
#else
  PAN_SERVO = speed * 2;
#endif
}

//Tilt angle alpha = atan(alt/dist) 
void calcTilt(){

  uint16_t alpha = 0;
  //prevent division by 0
  if (distance == 0){
    alpha = 90;
  }
  else {
    alpha = toDeg(atan((targetPosition.alt - trackerPosition.alt)/distance));
  }
  //just for current tests, later we will have negative tilt as well
  if (alpha < 0)
    alpha = 0;
    
  // same as arduinos map but a little bit faster ;)
  // Please note that this currently only works with positive tilt !!
  TILT_SERVO = (alpha * (TILT_90 - TILT_0) / 90 + TILT_0) * 2;
}

int digitalSmooth(int rawIn, int *sensSmoothArray){     // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int temp;
  uint8_t j, k, top, bottom;
  long total;
  static uint8_t i;
  static int sorted[filterSamples];
  boolean done;

  i = (i + 1) % filterSamples;    // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn;                 // input new data into the oldest slot

  for (j=0; j<filterSamples; j++){     // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (filterSamples - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }
  bottom = max(((filterSamples * 15)  / 100), 1); 
  top = min((((filterSamples * 85) / 100) + 1  ), (filterSamples - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}

uint16_t getHeading(geoCoordinate_t *a, geoCoordinate_t *b)
{
  // get difference between both points
  int32_t lat = a->lat - b->lat;
  int32_t lon = a->lon - b->lon;
  
  // calculate angle in radians and convert to degrees
  int16_t angle = atan2(lat, lon) * (180 / PI);
  
  // shift from -180/180 to 0/359
  return (uint16_t)(angle < 0 ? angle + 360 : angle);
}

#ifdef LOCAL_GPS
void initGps(){
  #ifdef MTK
    //set to 1Hz
    gpsSerial.println("$PMTK220,1000*1F");
    //delay for ack/nack as softserial cannot read and write at the same time
    delay(20);

    //only enable GGA sentences
    gpsSerial.print("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29");
    //delay for ack/nack as softserial cannot read and write at the same time
    delay(20);
  #endif
}
#endif

